﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;

namespace TreeLayoutPlayground
{
    class SpringLayoutEngine : LayoutEngine
    {
        public override void Layout(Tree tree)
        {
            SpringSystem system = new SpringSystem(tree);
            long steps = system.Run(0.001);
            Console.WriteLine("Steps: {0}", steps);

            system.UpdateTreePositions();

            tree.OverallSize = new Size(4000, 4000);
        }

        private class SpringSystem
        {
            struct Vector
            {
                public static readonly Vector XAxis = new Vector { X = 1, Y = 0 };
                public static readonly Vector YAxis = new Vector { X = 0, Y = 1 };

                public double X { get; set; }
                public double Y { get; set; }

                public double Length { get { return Math.Sqrt((X * X) + (Y * Y)); } }

                public void Normalize()
                {
                    double l = Length;
                    X /= l;
                    Y /= l;
                }

                public double Distance(Vector o)
                {
                    double dx = X - o.X;
                    double dy = Y - o.Y;

                    return Math.Sqrt((dx * dx) + (dy * dy));
                }

                public static Vector operator -(Vector a)
                {
                    return new Vector
                    {
                        X = -a.X,
                        Y = -a.Y
                    };
                }

                public static Vector operator +(Vector a, Vector b)
                {
                    return new Vector
                    {
                        X = a.X + b.X,
                        Y = a.Y + b.Y
                    };
                }

                public static Vector operator -(Vector a, Vector b)
                {
                    return new Vector
                    {
                        X = a.X - b.X,
                        Y = a.Y - b.Y
                    };
                }

                public static Vector operator *(Vector a, double b)
                {
                    return new Vector
                    {
                        X = a.X * b,
                        Y = a.Y * b
                    };
                }

                public static Vector operator *(double b, Vector a)
                {
                    return new Vector
                    {
                        X = a.X * b,
                        Y = a.Y * b
                    };
                }

                public static Vector operator /(Vector a, double b)
                {
                    return new Vector
                    {
                        X = a.X / b,
                        Y = a.Y / b
                    };
                }
            }

            class Box
            {
                internal Tree node;
                internal Vector position;
                internal Vector size;
                internal double mass;
                internal Vector velocity;
                internal bool mobile;

                private Vector acceleration;

                internal Vector center { get { return position + (size / 2); } }

                internal Box(Tree n, bool m)
                {
                    node = n;
                    mobile = m;

                    position = new Vector();
                    velocity = new Vector();
                    acceleration = new Vector();

                    size = new Vector { X = n.MyBox.Width, Y = n.MyBox.Height };
                    mass = (size.X * size.Y) / 10;
                }

                internal void addForce(Vector f)
                {
                    acceleration += f / mass;
                }

                internal void update(double dt, Vector gravity, double drag)
                {
                    if (mobile)
                    {
                        velocity += dt * (acceleration + gravity);
                        position += dt * velocity;
                        velocity *= (1 - drag);
                    }

                    acceleration.X = acceleration.Y = 0;
                }

                internal void repel(Box o)
                {
                    Vector dir = position - o.position;
                    double length = dir.Length;
                    dir /= length;
                    if (position.X < o.position.X + o.size.X + 15 && position.X + size.X + 15 > o.position.X)
                    {
                        if (position.Y < o.position.Y + o.size.Y + 15 && position.Y + size.Y + 15 > o.position.Y)
                        {
                            double xl = Math.Min(position.X + size.X + 7.5, o.position.X - 7.5);
                            double xr = Math.Max(position.X + size.X + 7.5, o.position.X - 7.5);
                            double yl = Math.Min(position.Y + size.Y + 7.5, o.position.Y - 7.5);
                            double yr = Math.Max(position.Y + size.Y + 7.5, o.position.Y - 7.5);

                            double oa = (xr - xl) * (yr - yl);
                            double force = 1000 * oa;
                            addForce(force * dir);
                            o.addForce(-force * dir);
                        }
                    }
                    //Vector dir = position - o.position;
                    //double length = dir.Length;
                    //dir /= length;
                    //double force = 1000000 / (length * length);

                    //addForce(force * dir);
                    //o.addForce(-force * dir);
                }
            }

            class Spring
            {
                internal Box left;
                internal Vector leftOffset;
                internal Box right;
                internal Vector rightOffset;

                internal double constant;
                internal double restLength;

                internal void applyForce()
                {
                    Vector dir = (left.center + leftOffset) - (right.center + rightOffset);
                    double len = dir.Length;
                    dir /= len;

                    double force = -constant * (len - restLength);

                    left.addForce(force * dir);
                    right.addForce(-force * dir);
                }

                internal bool correct(double cor)
                {
                    Vector dir = (left.center + leftOffset) - (right.center + rightOffset);
                    double len = dir.Length;
                    dir /= len;

                    if (len > ((1 + cor) * restLength) || len < ((1 - cor) * restLength))
                    {
                        double err = len - restLength;

                        if(left.mobile && right.mobile)
                        {
                            right.position += 0.5f * err * dir;
                            left.position += 0.5f * err * dir;
                        }
                        else if (!left.mobile)
                        {
                            right.position += err * dir;
                        }
                        else if (!right.mobile)
                        {
                            left.position += err * dir;
                        }
                        else { return false; }

                        return true;
                    }

                    return false;
                }
            }

            double timestep = 0.1;
            Vector gravity = new Vector { X = 0, Y = 9.81 };
            double drag = 0.1;

            List<Box> boxes;
            List<Spring> springs;

            public SpringSystem(Tree tree)
            {
                boxes = new List<Box>();
                springs = new List<Spring>();

                // Add the tree somehow
                Queue<Box> work = new Queue<Box>();

                Box root = new Box(tree, false);
                boxes.Add(root);
                work.Enqueue(root);

                do
                {
                    Box cur = work.Dequeue();

                    double xshift = 0;
                    foreach (Tree c in cur.node.Children)
                    {
                        Box child = new Box(c, true);
                        child.position = cur.position + new Vector { X = xshift, Y = 15 + cur.size.Y };
                        xshift += child.size.X;

                        Spring spring = new Spring();
                        spring.left = cur;
                        spring.leftOffset = new Vector
                        {
                            X = cur.size.X / 2,
                            Y = cur.size.Y
                        };
                        spring.right = child;
                        spring.rightOffset = new Vector
                        {
                            X = child.size.X / 2,
                            Y = 0
                        };
                        spring.constant = 500;
                        spring.restLength = 15;

                        springs.Add(spring);

                        boxes.Add(child);
                        work.Enqueue(child);
                    }
                } while (work.Count > 0);

                //for (int a = 0; a < boxes.Count; a++)
                //{
                //    for (int b = a + 1; b < boxes.Count; b++)
                //    {
                //        Spring spring = new Spring();
                //        spring.left = boxes[a];
                //        spring.right = boxes[b];
                //    }
                //}
            }

            public double Step()
            {
                foreach (Spring s in springs) { s.applyForce(); }

                for (int a = 0; a < boxes.Count; a++)
                {
                    for (int b = a + 1; b < boxes.Count; b++)
                    {
                        boxes[a].repel(boxes[b]);
                    }
                }

                double totalEnergy = 0;
                foreach (Box b in boxes)
                {
                    b.update(timestep, gravity, drag);

                    double speed = b.velocity.Length;
                    totalEnergy += 0.5 * b.mass * speed * speed;
                }

                return totalEnergy;
            }

            public long Run(double minEnergy)
            {
                long steps = 0;
                double curEnergy;
                do
                {
                    steps++;
                    curEnergy = Step();
                    //Console.WriteLine("Energy: {0}", curEnergy);
                } while (curEnergy > minEnergy);

                return steps;
            }

            public void UpdateTreePositions()
            {
                int leftmost = int.MaxValue;
                int topmost = int.MaxValue;
                // Set the positions
                foreach (Box b in boxes)
                {
                    b.node.MyRelativeTranslation = Size.Empty;
                    b.node.MyBox.X = (int)b.position.X;
                    b.node.MyBox.Y = (int)b.position.Y;

                    Console.WriteLine("({0}, {1})", b.position.X, b.position.Y);

                    leftmost = Math.Min(leftmost, b.node.MyBox.X);
                    topmost = Math.Min(topmost, b.node.MyBox.Y);
                }

                // Shift everything so the leftmost and topmost are the edges
                foreach (Box b in boxes)
                {
                    b.node.MyBox.Offset(-leftmost, -topmost);
                }
            }
        }
    }
}
